/*
 * Author: Anthony Fleshner
 * 
 * 
 */

import java.awt.Color;
import java.awt.Dimension;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.util.ArrayList;

import javax.swing.BorderFactory;
import javax.swing.JPanel;

import Jama.Matrix;

/*
 * class RobotComponent:
 * Handles drawing the different links.
 * Gets data from LinkMatrices.
 */
public class RobotComponent extends JPanel{
	
	Line link1;
	Line link2;
	Line link3;
	
    // holds circles drawn when isPaint is true
	ArrayList<Integer> circles;
	
	private boolean isPaint;
	private boolean isMouseMove;
    LinkMatrices kineMatrices;

    // change the below values to adjust movement amounts
    final double MOVE_LENGTH = 10;    // pixels
    final double ROTATE_ANGLE = 0.017; // radians
	
	public RobotComponent(){
		this.setBorder(BorderFactory.createLineBorder(Color.black));
		this.setSize(new Dimension(650,550));
		this.setPreferredSize(new Dimension(650,550));
		this.setVisible(true);
		
		link1 = new Line(325,500,325,350);
		link2 = new Line(325,350,325,250);
		link3 = new Line(325,250,325,175);
		
		isPaint = false;
		isMouseMove = false;
		
		circles = new ArrayList<Integer>();

        kineMatrices = new LinkMatrices(link1, link2, link3);
	}
	
    // draws the robot + its painting
	@Override
	public void paintComponent(Graphics g){
		super.paintComponent(g);
		Graphics2D g2d = (Graphics2D) g;
		
		g2d.setColor(Color.black);
		g2d.drawLine(0, 500, 650, 500);
		
		g2d.setColor(Color.red);
		g2d.drawLine(link1.getStartX(), link1.getStartY(), link1.getEndX(), link1.getEndY());
		g2d.setColor(Color.blue);
		g2d.drawLine(link2.getStartX(), link2.getStartY(), link2.getEndX(), link2.getEndY());
		g2d.setColor(Color.green);
		g2d.drawLine(link3.getStartX(), link3.getStartY(), link3.getEndX(), link3.getEndY());
		
		g2d.setColor(Color.black);
		if(isPaint){
			circles.add(link3.getEndX());
			circles.add(link3.getEndY());
		}
			for(int x = 0; x < circles.size(); x+=2)
				g2d.fillOval(circles.get(x)-5, circles.get(x+1)-5, 10, 10);
	}
	
	public void togglePaint(){
		isPaint = !isPaint;
	}
	
	public void setPaint(boolean value){
		isPaint = value;
	}
	
	public boolean isMouseMove(){
		return isMouseMove;
	}
	
	public void setMouseMove(){
		isMouseMove = !isMouseMove;
	}
	
	public LinkMatrices getLinkMatrices(){
		return kineMatrices;
	}

    public Line getLink1() {
        return link1;
    }

    // grab the updated links. should only be called by the move/rotate functions
    public void getNewLinks() {
        link1 = kineMatrices.getLink1();
        link2 = kineMatrices.getLink2();
        link3 = kineMatrices.getLink3();
    }

    // if possible, move the robot left
    public void moveLeft() {
        if (link1.getStartX() - MOVE_LENGTH < 175)
            return;
        kineMatrices.slide(-MOVE_LENGTH);
        getNewLinks();
    }

    // if possible, move the robot right
    public void moveRight() {
        if (link1.getStartX() + MOVE_LENGTH > 475)
            return;
        kineMatrices.slide(MOVE_LENGTH);
        getNewLinks();
    }

    // rotate link 2 clockwise
    public void rotate2CW() {
        kineMatrices.rot2(ROTATE_ANGLE);
        getNewLinks();
    }

    // rotate link 2 counterclockwise
    public void rotate2CCW() {
        kineMatrices.rot2(-ROTATE_ANGLE);
        getNewLinks();
    }

    // rotate link 3 clockwise
    public void rotate3CW() {
        kineMatrices.rot3(ROTATE_ANGLE);
        getNewLinks();
    }

    // rotate link 3 counterclockwise
    public void rotate3CCW() {
        kineMatrices.rot3(-ROTATE_ANGLE);
        getNewLinks();
    }

    public void invMoveLeft() {
        System.out.println("z: " + (-175+(link3.getEndX() - MOVE_LENGTH)));
        kineMatrices.inverse_kinematics(-175+(link3.getEndX() - MOVE_LENGTH), 500-link3.getEndY());
        getNewLinks();
    }

    public void invMoveRight() {
        kineMatrices.inverse_kinematics(-175+(link3.getEndX() + MOVE_LENGTH), 500-link3.getEndY());
        getNewLinks();
    }

    public void invMoveUp() {
        kineMatrices.inverse_kinematics(-175+link3.getEndX(), 500-(link3.getEndY() - MOVE_LENGTH));
        getNewLinks();
    }

    public void invMoveDown() {
        kineMatrices.inverse_kinematics(-175+link3.getEndX(), 500-(link3.getEndY() + MOVE_LENGTH));
        getNewLinks();
    }
    
    public void clear() {
    	circles.clear();
    }
    
    public void reset() {
		link1 = new Line(325,500,325,350);
		link2 = new Line(325,350,325,250);
		link3 = new Line(325,250,325,175);
		 kineMatrices = new LinkMatrices(link1, link2, link3);
    }
}
